/*
===========================================================================
This source file is part of DyLab (Dynamic Laboratory)
For the latest info, see http://dylab-modules.googlecode.com

Copyright (c) 2006-2008 Lukas Krejci
(krejci.lukas@volny.cz)

This file is part of DyLab.

    DyLab is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    DyLab is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with DyLab.  If not, see <http://www.gnu.org/licenses/>.
===========================================================================
*/

//===========================================================================================
#include <dylab/simobject/SimObject.h>
#include <dylab/simobject/masspoints/MassPointsStructureBase.h>
#include "Verlet_ModelIntegrator.h"

//===========================================================================================
const dylab::String dylab::Verlet_ModelIntegrator::COMPONENT_NAME = "verlet";

//===========================================================================================
dylab::Verlet_ModelIntegrator::Verlet_ModelIntegrator()
: ComModelIntegrator(COMPONENT_NAME)
, mForceClampingEnabled(false), mForceThresholdMin(DYLAB_R(0.001)), mForceThresholdMax(DYLAB_R(10))
{
}
//-----------------------------------------------------------------------------
dylab::Verlet_ModelIntegrator::~Verlet_ModelIntegrator()
{
}
//-----------------------------------------------------------------------------
void dylab::Verlet_ModelIntegrator::initialize(SimObject * _parentSimObject) 
{
    // check the argument
    DYLAB_ASSERT(_parentSimObject != NULL);
    mParentSimObject = _parentSimObject;

    // lock to retrieve mass points interface to initialize their history (we need two previous mass point's state)
    MassPointsStructureBase::ScopedExLock(mParentSimObject->getModelStructure())->reserveHistoryStates(2);
}
//-----------------------------------------------------------------------------
void dylab::Verlet_ModelIntegrator::loadFromPropertyTree(const PropertyTree & _pt)
{
    const PropertyTree::Node * forceClamping = _pt.findNode("forceClamping");
    if (forceClamping != NULL)
    {
        setForceClampingEnabled(forceClamping->getValue().parse<bool>());
        setForceThresholdMin(forceClamping->getChildNodeValueT("thresholdMin", getForceThresholdMin()));
        setForceThresholdMax(forceClamping->getChildNodeValueT("thresholdMax", getForceThresholdMax()));    
    }
}
//-----------------------------------------------------------------------------
dylab::SimObject * dylab::Verlet_ModelIntegrator::getParentSimObject()
{
    return mParentSimObject;
}
//-----------------------------------------------------------------------------
const dylab::SimObject * dylab::Verlet_ModelIntegrator::getParentSimObject() const
{
    return mParentSimObject;
}
//-----------------------------------------------------------------------------
dylab::String dylab::Verlet_ModelIntegrator::getDescription() const
{
	return String::fmt("%1% (%2%: %3% - %4%)", getFullName(), mForceClampingEnabled, mForceThresholdMin, mForceThresholdMax);
}
//-----------------------------------------------------------------------------
void dylab::Verlet_ModelIntegrator::timeStep(real_t _stepSize)
{   
    // precompute the squared step size that is constant through the loop
    real_t h2 = _stepSize * _stepSize;

	// this value will be needed in the following calls of method correctPosition
    mLastDoubleTimeStep = 2 * _stepSize;    

    // lock to retrieve structure pointer
    ComModelStructure::ScopedExLock structure(mParentSimObject->getModelStructure());

    // for each structure massPoint process the verlet integration algorithm to obtain new position (and velocity)    
	for (MassPointContainer::Iterator mp(MassPointsStructureBase::getImplFrom(structure)->massPointIterator()); 
			mp.isValid(); mp++)
    {                
        // skip fixed massPoint (they do not move at all)
        if (mp->flags().test(MassPoint::F_FIXED))
            continue;

        // if the forces can be clamped
        if (mForceClampingEnabled)
        {
            real_t forceMag = mp->currState().force.length();
            if (forceMag < mForceThresholdMin)
            {
                //DYLAB_TRACEF("Clamping force magnitude %1% to zero.", forceMag);
                mp->currState().force = Vector3::ZERO;
                continue; // no need to re-calc position, if the force is zero
            }
            else if (forceMag > mForceThresholdMax)
            {
                //DYLAB_TRACEF2("Clamping force magnitude %1% to max. threshold (%2%).", forceMag, mForceThresholdMax);
                mp->currState().force *= mForceThresholdMax / forceMag;
            }
        }

		// store the current mass point state
		mp->storeState();

        // compute new massPoint postion and velicity		
        mp->currState().position = mp->historyState(1).position * 2 - mp->historyState(2).position 
			+ mp->currState().force * (h2/mp->getMass());
		mp->currState().velocity = (mp->currState().position - mp->historyState(2).position) / mLastDoubleTimeStep;

		// reset the force
		mp->currState().force = Vector3::ZERO;
    }
}
//-----------------------------------------------------------------------------
void dylab::Verlet_ModelIntegrator::correctPosition(const Vector3 & _newPos, 
													MassPoint * _mp)
{
	// check the argument
	DYLAB_ASSERT(_mp != NULL);

	// store the new position
	_mp->currState().position = _newPos;

	// recompute the mass point's velocity vector
	_mp->currState().velocity = (_mp->currState().position - _mp->historyState(2).position) / mLastDoubleTimeStep;
}
//-----------------------------------------------------------------------------